/* ========================================================================== */
#include <float.h>

#include <util/common_macros.h>
#include <util/mem.h>
#include <util/gqsort.h>

#include "../matrix/matrix.h"
/* ========================================================================== */
static int * __create_int_sequence(int length, int start, int increment) {
	int i, inc;
	int * shift;

	shift = mem_allocn(length, int);
	for (i = 0, inc = start; i < length; i++, inc += increment) shift[i] = inc;

	return shift;
}
static int __compare(void * v1, void * v2) {

	real * r1 = (real *) v1;
	real * r2 = (real *) v2;

	if (*r1 < *r2) return -1;
	if (*r1 > *r2) return 1;
	return 0;
}
static void __dsp_order_filtering_without_mask(ImpMatrix * Mat, int order, ImpMatrix * Domain, ImpMatrix * Out) {

	// =========================================================================
	real * mat = NULL;
	real * out = NULL;
	int rows, columns;
	int r, c, _r, i;

	mat = Mat->data;
	out = Out->data;
	rows = Mat->rows;
	columns = Mat->columns;

	// =========================================================================
	real * domain = NULL;
	int mrows, mcolumns;
	int mr, mc, _mr, mi;

	domain = Domain->data;
	mrows = Domain->rows;
	mcolumns = Domain->columns;

	// =========================================================================
	int rf, cf;
	int mrc = (mrows - 1) >> 1;
	int mcc = (mcolumns - 1) >> 1;
	int _2columns_minus_2 = (columns << 1) - 2;
	int _2row_minus_2 = (rows << 1) - 2;
	int * r_shift = __create_int_sequence(rows, 0, columns);
	int d;

	// =========================================================================
	int dsize = 0;
	for (i = 0; i < Domain->size; i++) {
		if (domain[i]) dsize++;
	}

	int iv;
	real vec[dsize];

	// =========================================================================
	for (r = 0, _r = r * columns; r < rows; r++, _r += columns) {
		for (c = 0; c < columns; c++) {
			i = _r + c;
			// = For each pixel of Mat do: =====================================

			iv = 0;

			for (mr = 0, _mr = mr * mcolumns; mr < mrows; mr++, _mr += mcolumns) {
				// = Reflect y boundary ========================================
				rf = r + (mr - mrc);
				if (rf < 0) rf = -rf;
				else if (rf >= rows) rf = _2row_minus_2 - rf;

				for (mc = 0; mc < mcolumns; mc++) {
					mi = _mr + mc;
					// = For each element in the mask do: ======================

					if (!domain[mi]) {
						vec[i] = 9999; // FLT_MAX;
						continue;
					}

					// = Reflect x boundary ====================================
					cf = c + (mc - mcc);
					if (cf < 0) cf = -cf;
					else if (cf >= columns) cf = _2columns_minus_2 - cf;

					d = r_shift[rf] + cf;

					// = Operations ============================================
					// = Store all considered elements
					vec[iv] = mat[d];
					iv++;
				}
			}

			// = Sort the elements =============================================
			//qsort_float(vec, dsize);
			gqsort(vec, dsize, sizeof (real), __compare);
			out[i] = vec[order];
		}
	}
	//mem_free(vec);
}
static void __dsp_order_filtering_with_mask(ImpMatrix * Mat, int order, ImpMatrix * Domain, ImpMatrix * Mask, ImpMatrix * Out) {

	// =========================================================================
	real * mat = NULL;
	real * out = NULL;
	real * mask = NULL;
	int rows, columns;
	int r, c, _r, i;

	mat = Mat->data;
	out = Out->data;
	mask = Mask->data;
	rows = Mat->rows;
	columns = Mat->columns;

	// =========================================================================
	real * domain = NULL;
	int mrows, mcolumns;
	int mr, mc, _mr, mi;

	domain = Domain->data;
	mrows = Domain->rows;
	mcolumns = Domain->columns;

	// =========================================================================
	int rf, cf;
	int mrc = (mrows - 1) >> 1;
	int mcc = (mcolumns - 1) >> 1;
	int _2columns_minus_2 = (columns << 1) - 2;
	int _2row_minus_2 = (rows << 1) - 2;
	int * r_shift = __create_int_sequence(rows, 0, columns);
	int d;

	// =========================================================================
	int dsize = 0;
	for (i = 0; i < Domain->size; i++) {
		if (domain[i]) dsize++;
	}

	int iv;
	real * vec;
	vec = mem_allocn(dsize, real);

	// =========================================================================
	for (r = 0, _r = r * columns; r < rows; r++, _r += columns) {
		for (c = 0; c < columns; c++) {
			i = _r + c;
			// = For each pixel of Mat do: =====================================

			if (!mask[i]) continue;

			iv = 0;

			for (mr = 0, _mr = mr * mcolumns; mr < mrows; mr++, _mr += mcolumns) {
				// = Reflect y boundary ========================================
				rf = r + (mr - mrc);
				if (rf < 0) rf = -rf;
				else if (rf >= rows) rf = _2row_minus_2 - rf;

				for (mc = 0; mc < mcolumns; mc++) {
					mi = _mr + mc;
					// = For each element in the mask do: ======================

					if (!domain[mi]) continue;

					// = Reflect x boundary ====================================
					cf = c + (mc - mcc);
					if (cf < 0) cf = -cf;
					else if (cf >= columns) cf = _2columns_minus_2 - cf;

					d = r_shift[rf] + cf;

					// = Operations ============================================
					// = Store all considered elements
					vec[iv] = mat[d];
					iv++;
				}
			}

			// = Sort the elements =============================================
			//gqsort(vec, dsize, sizeof (real), __compare);
			qsort_float((float *) vec, dsize);
			out[i] = vec[order];
		}
	}
}
void imp_dsp_order_filtering(ImpMatrix * Mat, int order, ImpMatrix * Domain, ImpMatrix * Mask, ImpMatrix ** Out) {
	_exit_if_fail(Mat);
	_exit_if_fail(Domain);
	_exit_if_fail(Out);
	_exit_if_fail(order >= 0 && order < Domain->size);

	if (*Out == NULL) (*Out) = imp_matrix_new(Mat->rows, Mat->columns);

	if (Mask) __dsp_order_filtering_with_mask(Mat, order, Domain, Mask, *Out);
	else __dsp_order_filtering_without_mask(Mat, order, Domain, *Out);

}
/* ========================================================================== */
